#include <thread>
#include <cstring>
#include <iostream>
#include <ros/ros.h>
#include <serial/serial.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <csignal>
#include<Eigen/Dense>

#include "printf_utils.h"
#include "math_utils.h"


nav_msgs::Odometry viobot_odom;
nav_msgs::Odometry mocap_odom;
nav_msgs::Odometry viobot_odom_in_mocap;
nav_msgs::Odometry viobot_init_in_mocap;
bool get_viobot_init_pose{false};


ros::Publisher pub_viobot_odom;
ros::Time time_last{0};
Eigen::Vector3d mocap_pos;
Eigen::Vector3d mocap_euler,viobot_euler;
Eigen::Quaterniond q_mocap;
double viobot_init_yaw_in_mocap;

Eigen::Vector3d pos_error;
double yaw_error;


Eigen::Vector3d Q_to_euler(Eigen::Quaterniond Q);
void viobot_odom_cb(const nav_msgs::OdometryConstPtr& msg);
void mocap_pos_cb(const geometry_msgs::PoseStampedConstPtr& msg);

int main(int argc,char **argv)
{
    ros::init(argc,argv,"mini_odom_error");
    ros::NodeHandle nh;
    ros::Rate rate(100);

    // 发布odom话题
    ros::Subscriber mocap_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>("/vrpn_client_node/uav1/pose", 10, mocap_pos_cb);
    ros::Subscriber viobot_odom_sub = nh.subscribe<nav_msgs::Odometry>("/sunray/robobaton_mini/odom", 10, viobot_odom_cb);

    pub_viobot_odom = nh.advertise<nav_msgs::Odometry>("", 10);

    while(ros::ok())
    {
        if(ros::Time::now() - time_last > ros::Duration(1))
        {
            cout << GREEN << ">>>>>>>>>>>>>> Vibot error: " << TAIL << endl;
            //固定的浮点显示
            cout.setf(ios::fixed);
            // setprecision(n) 设显示小数精度为n位
            cout << setprecision(2);
            //左对齐
            cout.setf(ios::left);
            // 强制显示小数点
            cout.setf(ios::showpoint);
            // 强制显示符号
            cout.setf(ios::showpos);

            pos_error[0] = viobot_odom.pose.pose.position.x - viobot_odom_in_mocap.pose.pose.position.x;
            pos_error[1] = viobot_odom.pose.pose.position.y - viobot_odom_in_mocap.pose.pose.position.y;
            pos_error[2] = viobot_odom.pose.pose.position.z - viobot_odom_in_mocap.pose.pose.position.z;

            yaw_error = viobot_euler[2] - viobot_odom_in_mocap.twist.twist.angular.z;

            cout << GREEN << "mini_pos  [X Y Z] : " << viobot_odom.pose.pose.position.x << " [ m ] " << viobot_odom.pose.pose.position.y << " [ m ] " << viobot_odom.pose.pose.position.z << " [ m ] " << TAIL << endl;
            cout << GREEN << "mini_att  [R P Y] : " << viobot_euler[0] * 180 / M_PI << " [deg] " << viobot_euler[1] * 180 / M_PI << " [deg] " << viobot_euler[2] * 180 / M_PI << " [deg] " << TAIL << endl;

            cout << GREEN << "mocap_pos [X Y Z] : " << mocap_odom.pose.pose.position.x << " [ m ] " << mocap_odom.pose.pose.position.y << " [ m ] " << mocap_odom.pose.pose.position.z << " [ m ] " << TAIL << endl;
            cout << GREEN << "mocap_att [R P Y] : " << mocap_euler[0] * 180 / M_PI << " [deg] " << mocap_euler[1]  * 180 / M_PI << " [deg] " << mocap_euler[2]  * 180 / M_PI << " [deg] " << TAIL << endl;

            cout << GREEN << "mini_mocap [X Y Z]: " << viobot_odom_in_mocap.pose.pose.position.x << " [ m ] " << viobot_odom_in_mocap.pose.pose.position.y << " [ m ] " << viobot_odom_in_mocap.pose.pose.position.z << " [ m ] " << TAIL << endl;
            cout << GREEN << "mini_mocap [R P Y]: " << viobot_odom_in_mocap.twist.twist.angular.z * 180 / M_PI << " [deg] " << TAIL << endl;

            cout << YELLOW<< "pos_error [X Y Z] : " << pos_error[0] << " [ m ] " << pos_error[1] << " [ m ] " << pos_error[2] << " [ m ] " << TAIL << endl;
            cout << YELLOW<< "yaw_error [     ] : " << yaw_error * 180 / M_PI << " [deg] " << TAIL << endl;

            time_last = ros::Time::now();
        }

        rate.sleep();
        ros::spinOnce(); 
    }

    return 0;
}

void viobot_odom_cb(const nav_msgs::OdometryConstPtr& msg)
{
    viobot_odom = *msg;

    Eigen::Quaterniond q = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
    // viobot_euler = quaternion_to_euler(q_mocap);
    // ?
    viobot_euler = Q_to_euler(q);
}

// 将四元数转换为旋转矩阵(外旋,x,y,z)
Eigen::Vector3d Q_to_euler(Eigen::Quaterniond Q)
{
    Eigen::Matrix3d R = Q.toRotationMatrix();
    // 提取外旋 X-Y-Z 欧拉角
    double theta = asin(-R(2, 0)); // Pitch
    double psi = atan2(R(1, 0), R(0, 0)); // Yaw
    double phi = atan2(R(2, 1), R(2, 2)); // Roll
    return Eigen::Vector3d(phi, theta, psi);
}

void mocap_pos_cb(const geometry_msgs::PoseStampedConstPtr& msg)
{
	mocap_pos[0] = msg->pose.position.x;
    mocap_pos[1] = msg->pose.position.y;
	mocap_pos[2] = msg->pose.position.z;

    q_mocap = Eigen::Quaterniond(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z);
    // mocap_euler = quaternion_to_euler(q_mocap);
    // ?
    mocap_euler = Q_to_euler(q_mocap);

    // mocap_odom
    mocap_odom.header.stamp = ros::Time::now();
    mocap_odom.header.frame_id = "mocap";
    mocap_odom.pose.pose.position.x = msg->pose.position.x;
    mocap_odom.pose.pose.position.y = msg->pose.position.y;
    mocap_odom.pose.pose.position.z = msg->pose.position.z;
    mocap_odom.pose.pose.orientation.x = msg->pose.orientation.x;
    mocap_odom.pose.pose.orientation.y = msg->pose.orientation.y;   
    mocap_odom.pose.pose.orientation.z = msg->pose.orientation.z;
    mocap_odom.pose.pose.orientation.w = msg->pose.orientation.w;
    mocap_odom.twist.twist.linear.x = 0.0;
    mocap_odom.twist.twist.linear.y = 0.0;
    mocap_odom.twist.twist.linear.z = 0.0;
    mocap_odom.twist.twist.angular.x = 0.0;
    mocap_odom.twist.twist.angular.y = 0.0;
    mocap_odom.twist.twist.angular.z = 0.0;

    if(!get_viobot_init_pose)
    {
        viobot_init_in_mocap = mocap_odom;
        viobot_init_yaw_in_mocap = mocap_euler[2];
        get_viobot_init_pose = true;
    }else
    {
        viobot_odom_in_mocap.header.stamp = mocap_odom.header.stamp;
        viobot_odom_in_mocap.header.frame_id = "viobot";
        viobot_odom_in_mocap.pose.pose.position.x = mocap_odom.pose.pose.position.x - viobot_init_in_mocap.pose.pose.position.x;
        viobot_odom_in_mocap.pose.pose.position.y = mocap_odom.pose.pose.position.y - viobot_init_in_mocap.pose.pose.position.y;
        viobot_odom_in_mocap.pose.pose.position.z = mocap_odom.pose.pose.position.z - viobot_init_in_mocap.pose.pose.position.z;
        viobot_odom_in_mocap.pose.pose.orientation.x = msg->pose.orientation.x;
        viobot_odom_in_mocap.pose.pose.orientation.y = msg->pose.orientation.y;   
        viobot_odom_in_mocap.pose.pose.orientation.z = msg->pose.orientation.z;
        viobot_odom_in_mocap.pose.pose.orientation.w = msg->pose.orientation.w;
        viobot_odom_in_mocap.twist.twist.linear.x = 0.0;
        viobot_odom_in_mocap.twist.twist.linear.y = 0.0;
        viobot_odom_in_mocap.twist.twist.linear.z = 0.0;
        viobot_odom_in_mocap.twist.twist.angular.x = 0.0;
        viobot_odom_in_mocap.twist.twist.angular.y = 0.0;
        // special
        viobot_odom_in_mocap.twist.twist.angular.z = mocap_euler[2] - viobot_init_yaw_in_mocap;
    }


}

